from machine import Pin, SoftI2C
# import ssd1306
# 
# display = ssd1306.SSD1306_I2C(128, 64, i2c)
# 
# display.text('Hello World', 0, 0, 1)
# display.show()

MPU6050_SMPLRT_DIV		=0x19
MPU6050_CONFIG			=0x1A
MPU6050_GYRO_CONFIG		=0x1B
MPU6050_ACCEL_CONFIG	=0x1C
MPU6050_ACCEL_XOUT_H	=0x3B
MPU6050_ACCEL_XOUT_L	=0x3C
MPU6050_ACCEL_YOUT_H	=0x3D
MPU6050_ACCEL_YOUT_L	=0x3E
MPU6050_ACCEL_ZOUT_H	=0x3F
MPU6050_ACCEL_ZOUT_L	=0x40
MPU6050_TEMP_OUT_H		=0x41
MPU6050_TEMP_OUT_L		=0x42
MPU6050_GYRO_XOUT_H		=0x43
MPU6050_GYRO_XOUT_L		=0x44
MPU6050_GYRO_YOUT_H		=0x45
MPU6050_GYRO_YOUT_L		=0x46
MPU6050_GYRO_ZOUT_H		=0x47
MPU6050_GYRO_ZOUT_L		=0x48
MPU6050_PWR_MGMT_1		=0x6B
MPU6050_PWR_MGMT_2		=0x6C
MPU6050_WHO_AM_I		=0x75


class Mpu6050:
    def __init__(self):
        self.i2c = SoftI2C(scl=Pin(5), sda=Pin(4), freq=100000)

                 # 电源管理1
         # 重置（复位）所有寄存器 
        self.write(MPU6050_PWR_MGMT_1,  0x80)

        # 关闭睡眠（唤醒）、使用陀螺仪的时钟
        self.write(MPU6050_PWR_MGMT_1,0x01)

        #电源管理寄存器2，保持默认值0，所有轴均不待机
        self.write(MPU6050_PWR_MGMT_2, 0x00)



        # 数字低通滤波器配置为6
        # 一旦配置了 低通滤波器，那么采样率就只能为 1kHz 而不是 8kHz
        self.write(MPU6050_CONFIG,0x06)

        # 配置采样率分频系数
        # 那么实际的采样率 = 1kHz/(1+4) = 200Hz
        self.write(MPU6050_SMPLRT_DIV,0x04)

        # 配置加速度量程为+- 2g
        # 并进行加速度计自检
        self.write(MPU6050_ACCEL_CONFIG,0x00)

        # 配置角速度传感器的量程为 +- 250°/s
        # 要使用DMP，则量程必须设置为 +- 2000°/s
        # 并进行陀螺仪三轴自检
        self.write(MPU6050_GYRO_CONFIG,0x00)

            
    def write(self,registerAddr,arg):
        self.i2c.start()
        
        self.i2c.write(bytes([0xd0]))
        
        self.i2c.write(bytes([registerAddr]))
        
        #for arg in args:
        self.i2c.write(bytes([arg]))
        
        self.i2c.stop()

    def read(self,registerAddr,buf):
        self.i2c.start()
        
        self.i2c.write(bytes([0xd0]))
        
        self.i2c.write(bytes([registerAddr]))
        
        self.i2c.start()
        
        self.i2c.write(bytes([0xd1]))
        
        
        
        self.i2c.readinto(buf) # , nack=True
         
        self.i2c.stop()
        
        return buf
    
    def read_6_axis_data(self,buf):
        return self.read(0x3b,buf)


